#include "mode_control.h"
#include "./hardware/io.h"
#include "./drive/sys_timer.h"
#include "face.h"
#include "motion.h"
#include "tts.h"
#include <string.h>
#define LEFT 0
#define RIGHT 1

#define LONG_TOUCH 500 // ms
SwitchInput touch_l;
SwitchInput touch_r;

uint32_t touch_buf;
uint8_t touch_inidex = 0;
uint8_t touch_start = 0;

uint32_t touch_wait = 0;
uint32_t touch_start_time[2];
uint32_t touch_time[2];

MODE_TYPE cur_mode, last_mode;
/*
左短按 - 无
左长按 - 
右短按 - 无
右长按 - 静态模式
双短按 - 表情演示
双长按 - 避障模式
左滑
右滑
*/

void ModeInit() {
  touch_l.Init(TOUCH_L);
  touch_r.Init(TOUCH_R);
  cur_mode = MODE_STATIC;
  last_mode = MODE_STATIC;
}

void PushTouch(uint8_t lr, uint8_t var) {
  if (touch_start == 0 && (var == 0)) {
    memset((char *)touch_time, 0, sizeof(touch_time));
    memset((char *)touch_start_time, 0, sizeof(touch_time));
    touch_start = 1;
    touch_wait = CurMillis() + 1000;
  }
  if (var == 0) {
    touch_start_time[lr] = CurMillis();
  } else {
    if (touch_start_time[lr])
      touch_time[lr] =  CurMillis() - touch_start_time[lr];
  }
}

void ModeSet(MODE_TYPE mode) {
  last_mode = cur_mode;
  cur_mode = mode;
  if ((cur_mode == MODE_RREMOTE) && (last_mode != MODE_RREMOTE)) {
    TTS(TTS_REMOTE_CTRL);
  }
}

void ModeTouchGet() {
  if (touch_start) {
    if (touch_wait < CurMillis()) {
      touch_start = 0;
      if (touch_time[LEFT] && touch_time[RIGHT]) {
        if ((touch_time[LEFT] > LONG_TOUCH) || (touch_time[RIGHT] > LONG_TOUCH)) {
          // 双健长按
          ModeSet(MODE_AVOID);
          MotionRest();
        } else {
          // 双键短按
          FaceTestMode();
        }
      } else if (touch_time[LEFT]) {
        if (touch_time[LEFT] > LONG_TOUCH) {
          // 左键长按
          ModeSet(MODE_STATIC);
          MotionRest();
        } else {
          // 左键短按
        }
      } else if (touch_time[RIGHT]) {
        if (touch_time[RIGHT] > LONG_TOUCH) {
          // 右键长按
        } else {
          // 右键短按
        }
      }
    }
  }
  if (touch_l.CheckStatuLoop()) {
    PushTouch(LEFT, touch_l.Read());
  }
  if (touch_r.CheckStatuLoop()) {
    PushTouch(RIGHT, touch_r.Read());
  }
}

void ModeLoop() {
  ModeTouchGet();
  switch (cur_mode) {
    case MODE_STATIC:
      MotionStaticProcess();
      break;
    case MODE_FACE_TEST:
      FaceTestMode();
      break;
    case MODE_AVOID:
      TTS(TTS_AVOID);
      MotionAvoidProcess();
      break;
    case MODE_RREMOTE:
      MotionRemoteProcess();
      break;
  }
}